from datetime import datetime
from pathlib import Path
import struct

"""
    coommad_utils.py

    @description:
        指令工具类
    @author:
        ZhenYu
    @date:
        2024/6/6
"""

'''
5 mode 指令模式
6-9 维度
10-13 经度
14-15 GPS
16校验和
'''


def get_send_full_command(machineType: str, command, uav_id, camera_id) -> list:
    byte1 = 0x9F
    byte2 = 0x00
    full_command = []

    uav_id = int(uav_id)
    camera_id = int(camera_id)

    machineType = str(machineType)
    if machineType == "MCManger":
        byte2 = 0xE7
    elif machineType == "CompoundEye":
        byte2 = 0xE4
    full_command.append(byte1)  # 1 同步头
    full_command.append(byte2)  # 2 数据表示
    full_command.append(uav_id << 16 >> 16)  # 3-4 备用 无人机id
    full_command.append(camera_id << 16 >> 16)  # 摄像头id
    full_command.append(command)  # mode
    full_command.append(camera_id << 16 >> 16)  # 摄像头id
    full_command[7:9] = [0x00, 0x00, 0x00]
    full_command[10:13] = [0x00, 0x00, 0x00, 0x00]
    full_command[14:15] = [0x00, 0x00]
    full_command.append(CheckSum(full_command, 0, len(full_command) - 1))

    full_command = ['0x{:02X}'.format(num) for num in full_command]

    return full_command


def List_Hex(data: list) -> str:
    list = ['0x{:02X}'.format(num) for num in data]
    return str(list)


def Bytes_HexStr(data: bytes) -> str:
    """
    十六进制数组转字符串

    :param data:   二进制数组 byte_data = b'\x9F\xE4\x00\x01\xFD'
    :return: 十六进制字符串    9FE40001FD
    """
    hex_str = ''.join(f'{byte:02x}' for byte in data).upper()
    return hex_str


def get_test_command(command_size: int) -> list:
    byte1 = 0x9F
    byte2 = 0xE4
    test_command = []

    test_command.append(byte1)  # 1 同步头
    test_command.append(byte2)

    for i in range(0, command_size - 2 - 1):
        test_command.append(i)
    test_command.append(CheckSum(test_command, 0, len(test_command) - 1))
    test_command = ['0x{:02X}'.format(num) for num in test_command]
    return test_command


def CheckSum(data, start, end):
    """
    计算校验和
    :param data:    计算校验和的数组
    :param start:
    :param end:
    :return:
    """
    checksum = 0x00
    for i in range(start, end):
        checksum += data[i]
    if checksum > 0xff:
        checksum = checksum & 0x000000FF
    return checksum


def data_96bytes_parser(driver_type: str, temp_data, save_file_index: int) -> (int, int):
    """
    96字节复眼回传指令解析
    解析有效数据保存至data/rgb或thermal_fly_data目录下
    文件存储格式为txt，根据driver_type与index进行保存
    :param driver_type: 载荷类型 rgb/thermal
    :param temp_data: 96字节数据
    :param save_file_index: 保存文件的序号
    :return:   目标数量， 摄像头序号
    """

    if temp_data[0] == 0x9F and temp_data[1] == 0xE4 and temp_data[2] == 0x9F and temp_data[3] == 0xE4:
        # TODO 如果测试时，数据不正确，将pis pit等数组中的0和1交换位置
        #   可以直接接不使用unpack

        ########################################
        # fly data 64 bytes                    #
        ########################################

        index_low = 1
        index_high = 0

        # 航向角 psi 直接对低八位进行运算
        # psi = struct.unpack('bb', temp_data[22 - 1:23])
        psi = temp_data[22 - 1:23]
        # psi = psi[index_low] << 8 | psi[index_high]
        psi = (psi[index_low] & 0x0F) << 8 | psi[index_high]
        psi = psi / 10.0 + 10.0
        if psi > 360.0:
            psi = psi - 360.0

        # 俯仰角 pit
        # pit = struct.unpack('bb', temp_data[24 - 1:25])
        pit = temp_data[24 - 1:25]
        pit = pit[index_low] << 8 | pit[index_high]
        pit = pit * 156.0 / 4095.0 - 78.0

        # 倾斜角 rol
        # rol = struct.unpack('bb', temp_data[26 - 1:27])
        rol = temp_data[26 - 1:27]
        rol = rol[index_low] << 8 | rol[index_high]
        rol = rol * 156.0 / 4095.0 - 78.0

        # 空速 v 0.01m/s
        # v = struct.unpack('bb', temp_data[28 - 1:29])
        v = temp_data[28 - 1:29]
        v = v[index_low] << 8 | v[index_high]
        v = v / 100.0

        # 气压高度
        # h = struct.unpack('bb', temp_data[30 - 1:31])
        h = temp_data[30 - 1:31]
        h = h[index_low] << 8 | h[index_high]
        h = h * 2.5 - 300

        # GPS地速 0.01m/s
        # v_gps = struct.unpack('bb', temp_data[32 - 1:33])
        v_gps = temp_data[32 - 1:33]
        v_gps = v_gps[index_low] << 8 | v_gps[index_high]
        v_gps = v_gps / 100.0

        # 维度 0.001 首位为标志位 保留9位小数点
        lat = temp_data[34 - 1:37]
        bits_lat_1 = lat[3] & 0x80 != 0
        if bits_lat_1 is True:  # 首位0
            lat = lat[3] << 24 | lat[2] << 16 | lat[1] << 8 | lat[0]
            lat = -(lat / 1000.0) / 3600.0
            lat = format(lat, '.9f')
        else:  # 首位1 改为0在计算 带上+
            buffer = bytearray(lat)
            buffer[3] &= 0x7F
            lat = buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]
            lat = lat / 1000.0 / 3600.0
            lat = format(lat, '.9f')

        lon = temp_data[38 - 1:41]
        bits_lon_1 = lon[3] & 0x80 != 0
        if bits_lon_1 is True:  # 首位0
            lon = lon[3] << 24 | lon[2] << 16 | lon[1] << 8 | lon[0]
            lon = -(lon / 1000.0) / 3600.0
            lon = format(lon, '.9f')
        else:  # 首位1 改为0在计算 带上+
            buffer = bytearray(lon)
            buffer[3] &= 0x7F
            lon = buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]
            lon = lon / 1000.0 / 3600.0
            lon = format(lon, '.9f')

        # GPS高度
        # H_gps = struct.unpack('bb', temp_data[42 - 1:43])
        H_gps = temp_data[42 - 1:43]
        H_gps = H_gps[index_low] << 8 | H_gps[index_high]
        H_gps = H_gps * 1 - 1000

        # 航迹角
        # Psi_gs = struct.unpack('bb', temp_data[44 - 1:45])
        Psi_gs = temp_data[44 - 1:45]
        Psi_gs = Psi_gs[index_low] << 8 | Psi_gs[index_high]
        Psi_gs = Psi_gs / 10.0

        # 年月日
        year = temp_data[46 - 1]
        month = temp_data[47 - 1]
        day = temp_data[48 - 1]

        # 时分秒 +8改为utc时间
        hour = temp_data[49 - 1] + 8
        minute = temp_data[50 - 1]
        second = temp_data[51 - 1]

        # 起飞点海拔
        BY = temp_data[52 - 1:53]
        BY = BY[index_low] << 8 | BY[index_high]

        # 无人机东向速度
        v_east = struct.unpack('bb', temp_data[58 - 1:59])
        v_east = v_east[index_low] << 8 | v_east[index_high]
        v_east = v_east * 200 / 32768

        # 无人机北向速度
        v_north = struct.unpack('bb', temp_data[60 - 1:61])
        v_north = v_north[index_low] << 8 | v_north[index_high]
        v_north = v_north * 200 / 32768

        # 无人机天向速度
        v_up = struct.unpack('bb', temp_data[62 - 1:63])
        v_up = v_up[index_low] << 8 | v_up[index_high]
        v_up = v_up * 200 / 32768

        fly_data_by = temp_data[64 - 1]

        ########################################
        # CompoundEye 32 bytes                 #
        ########################################

        compound_eye_flag = temp_data[64 - 1]
        eye_index_low = 1
        eye_index_high = 0

        if compound_eye_flag == 0xAA:
            pass

        # 拼接图像宽
        compact_image_width = struct.unpack('bb', temp_data[65 - 1:66])
        compact_image_width = compact_image_width[eye_index_low] << 8 | compact_image_width[eye_index_high]

        # 拼接图像高
        compact_image_height = struct.unpack('bb', temp_data[67 - 1:68])
        compact_image_height = compact_image_height[eye_index_low] << 8 | compact_image_height[eye_index_high]

        # 拼接图像像素点坐标原点 0x01左上 0x10左下
        compact_image_origin = temp_data[69 - 1]

        # 目标数量
        target_num = temp_data[70 - 1]

        # 1号目标信息
        target_1_info = temp_data[71 - 1]

        # 1号目标左上X
        target_1_left_top_x = struct.unpack('bb', temp_data[72 - 1:73])
        target_1_left_top_x = target_1_left_top_x[eye_index_low] << 8 | target_1_left_top_x[eye_index_high]

        # 1号目标左上Y
        target_1_left_top_y = struct.unpack('bb', temp_data[74 - 1:75])
        target_1_left_top_y = target_1_left_top_y[eye_index_low] << 8 | target_1_left_top_y[eye_index_high]

        # 1号目标右下X
        target_1_right_bottom_x = struct.unpack('bb', temp_data[76 - 1:77])
        target_1_right_bottom_x = target_1_right_bottom_x[eye_index_low] << 8 | target_1_right_bottom_x[eye_index_high]

        # 1号目标右下Y
        target_1_right_bottom_y = struct.unpack('bb', temp_data[78 - 1:79])
        target_1_right_bottom_y = target_1_right_bottom_y[eye_index_low] << 8 | target_1_right_bottom_y[eye_index_high]

        # 2号目标
        target_2_info = temp_data[80 - 1]

        # 2号目标左上X
        target_2_left_top_x = struct.unpack('bb', temp_data[81 - 1:82])
        target_2_left_top_x = target_2_left_top_x[eye_index_low] << 8 | target_2_left_top_x[eye_index_high]

        # 2号目标左上Y
        target_2_left_top_y = struct.unpack('bb', temp_data[83 - 1:84])
        target_2_left_top_y = target_2_left_top_y[eye_index_low] << 8 | target_2_left_top_y[eye_index_high]

        # 2号目标右下X
        target_2_right_bottom_x = struct.unpack('bb', temp_data[85 - 1:86])
        target_2_right_bottom_x = target_2_right_bottom_x[eye_index_low] << 8 | target_2_right_bottom_x[eye_index_high]

        # 2号目标右下Y
        target_2_right_bottom_y = struct.unpack('bb', temp_data[87 - 1:88])
        target_2_right_bottom_y = target_2_right_bottom_y[eye_index_low] << 8 | target_2_right_bottom_y[eye_index_high]

        # 获取摄像头编号
        camera_id = temp_data[90]

        title_list = ["psi", "pit", "rol", "v", "h", "v_gps", "lat", "lon", "H_gps", "Psi_gs", "year", "month", "day",
                      "hour", "minute", "second", "BY", "v_east", "v_north", "v_up",
                      "compact_image_width", "compact_image_height", "compact_image_origin", "target_num",
                      "target_1_info", "target_1_left_top_x", "target_1_left_top_y", "target_1_right_bottom_x",
                      "target_1_right_bottom_y",
                      "target_2_info", "target_2_left_top_x", "target_2_left_top_y", "target_2_right_bottom_x",
                      "target_2_right_bottom_y", "camrea_id"
                      ]

        data_list = [psi, pit, rol, v, h, v_gps, lat, lon, H_gps, Psi_gs, year, month, day, hour, minute, second,
                     BY, v_east, v_north, v_up,
                     compact_image_width, compact_image_height, compact_image_origin, target_num,
                     target_1_info, target_1_left_top_x, target_1_left_top_y, target_1_right_bottom_x,
                     target_1_right_bottom_y,
                     target_2_info, target_2_left_top_x, target_2_left_top_y, target_2_right_bottom_x,
                     target_2_right_bottom_y, camera_id]

        # 32字节数据
        # print(data_list[20-1 : len(data_list)-1])
        data_dict = {key: value for key, value in zip(title_list, data_list)}
        # print(data_dict)

        # 文件保存 保存至主目录的data目录下的 rgb或thermal数据中
        current_path = Path(__file__).resolve()
        parent_path = current_path.parent.parent.parent
        folder = parent_path / "data"
        new_file_name = ""

        if driver_type.lower() == "rgb":
            folder = folder / "rgb_fly_data"
            new_file_name = "rgb_fly_data_" + str(save_file_index) + ".txt"
        if driver_type.lower() == "thermal":
            folder = folder / "thermal_fly_data"
            new_file_name = "thermal_fly_data_" + str(save_file_index) + ".txt"
        if driver_type.lower() == "mc":
            folder = folder / "mc_origin_fly_data"
            new_file_name = "mc_origin_fly_data_" + str(save_file_index) + ".txt"

        if not folder.exists():
            folder.mkdir()
        new_file = folder / new_file_name
        if not new_file.exists():
            new_file.touch()
        # print(new_file)
        with open(new_file, "w") as file:
            for key, value in data_dict.items():
                file.write(f"{key}:{value}\n")
            file.write(Bytes_HexStr(temp_data))

        return target_num, camera_id


def get_current_time() -> str:
    # 获取当前时间
    current_time = datetime.now()
    # 格式化当前时间为指定格式的字符串
    formatted_time = current_time.strftime("%Y-%m-%d %H:%M:%S")
    return formatted_time


if __name__ == '__main__':
    list = get_send_full_command("MCManger", 0x80, 2, 6)
    print(list)

    list = get_test_command(32)
    hex_string = ''.join([hex_value[2:] for hex_value in list])

    print(hex_string)
    print(len(list))

    byte_data = b'\x9F\xE4\x00\x01\xFD'
    hex_str = Bytes_HexStr(byte_data)

    # 打印结果
    print(hex_str)

    hex_str = "9FE49FE4000020C0ED7C118053CF110000900000009E10E60BD20AA00F5800380EC0ED7C118053CF110000BA00551501142E3370000525E401C22DF122F90EAA001EE0100101015A017400C20438040000000000000000000000000000000206".upper()
    byte_seq = bytes.fromhex(hex_str)
    print(byte_seq)

    target_num, camera_id = data_96bytes_parser("thermal", byte_seq, 1)
    print(f"target_num:{target_num}")
    print(f"camera_id:{camera_id}")
